/**
 * Comprehensive BLDC motor control example using Hall sensor
 * 
 * Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
 * - configure PID controller constants
 * - change motion control loops
 * - monitor motor variabels
 * - set target values
 * - check all the configuration values 
 * 
 * See more info in docs.simplefoc.com/commander_interface
 */
#include <SimpleFOC.h>
// software interrupt library
#include <PciManager.h>
#include <PciListenerImp.h>

// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(11);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 5, 6, 8);
// Stepper motor & driver instance
//StepperMotor motor = StepperMotor(50);
//StepperDriver4PWM driver = StepperDriver4PWM(9, 5, 10, 6,  8);

// hall sensor instance
HallSensor sensor = HallSensor(2, 3, 4, 11);

// Interrupt routine intialisation
// channel A, B and C callbacks
void doA()
{ sensor.handleA(); }

void doB()
{ sensor.handleB(); }

void doC()
{ sensor.handleC(); }

// If no available hadware interrupt pins use the software interrupt
PciListenerImp listenC(sensor.pinC, doC);


// commander interface
Commander command = Commander(Serial);

void onMotor(char *cmd)
{ command.motor(&motor, cmd); }


void setup()
{

    // initialize encoder sensor hardware
    sensor.init();
    sensor.enableInterrupts(doA, doB); //, doC);
    // software interrupts
    PciManager.registerListener(&listenC);
    // link the motor to the sensor
    motor.linkSensor(&sensor);

    // driver config
    // power supply voltage [V]
    driver.voltage_power_supply = 12;
    driver.init();
    // link driver
    motor.linkDriver(&driver);


    // choose FOC modulation
    motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
    // set control loop type to be used
    motor.controller = MotionControlType::torque;
    // contoller configuration based on the controll type
    motor.PID_velocity.P = 0.2;
    motor.PID_velocity.I = 20;
    motor.PID_velocity.D = 0;
    // default voltage_power_supply
    motor.voltage_limit = 12;

    // velocity low pass filtering time constant
    motor.LPF_velocity.Tf = 0.01;

    // angle loop controller
    motor.P_angle.P = 20;
    // angle loop velocity limit
    motor.velocity_limit = 50;

    // use monitoring with serial for motor init
    // monitoring port
    Serial.begin(115200);
    // comment out if not needed
    motor.useMonitoring(Serial);

    // initialise motor
    motor.init();
    // align encoder and start FOC
    motor.initFOC();

    // set the inital target value
    motor.target = 2;

    // define the motor id
    command.add('A', onMotor, "motor");

    // Run user commands to configure and the motor (find the full command list in docs.simplefoc.com)
    Serial.println(F("Motor commands sketch | Initial motion control > torque/voltage : target 2V."));

    _delay(1000);
}


void loop()
{
    // iterative setting FOC phase voltage
    motor.loopFOC();

    // iterative function setting the outter loop target
    // velocity, position or voltage
    // if tatget not set in parameter uses motor.target variable
    motor.move();

    // user communication
    command.run();
}


